import sys
from queue import PriorityQueue
from typing import Dict, List

import numpy as np

from agent import Agent
from wall import Exit


class AgentExit:

    def __init__(self):
        self.num: int = 0
        self.position: np.ndarray = np.array([0, 0])
        self.score: float = sys.float_info.max


class FindExits:

    def __init__(self, agents_hashmap: Dict[int, Agent], exits: List[Exit]):
        self.c = 1
        self.agents_hashmap: Dict[int, Agent] = agents_hashmap
        self.exits: List[Exit] = exits
        self.agents_exit: List[AgentExit] = [AgentExit() for _ in range(len(agents_hashmap))]

    def choose_exit(self):
        for e in self.exits:
            priority_queue = PriorityQueue()

            for name, agent in self.agents_hashmap.items():
                priority_queue.put((np.linalg.norm(e.position - agent.position), name))

            closer_agents: int = 0
            while not priority_queue.empty():
                item = priority_queue.get()
                score = self.cal_score(item[0], self.agents_hashmap[name].max_speed, e.width, closer_agents)
                if score < self.agents_exit[name].score:
                    self.agents_exit[name].score = score
                    self.agents_exit[name].num = e.num
                    self.agents_exit[name].position = e.position
                closer_agents += 1

    def cal_score(self, dis: float, v: float, width: float, closer_agents: int) -> float:
        return dis / v + self.c * closer_agents / width


if __name__ == '__main__':
    pass
